function [xOut, mse] = selfLocSigmaPoints(initalisation, parameters, topology, measurements)

%--------------------get parameters and data from structures----------------------------
N = parameters.N;
T = parameters.MPiter;
A = parameters.A;
W = parameters.W;
var_v = parameters.varV;
var_m = parameters.varM;
alpha = parameters.sigmapar.alpha;
beta = parameters.sigmapar.beta;

meanInit = initalisation.mean;
covInit = initalisation.cov;

con_ndac = topology.conNdAc;
con_ndnd = topology.conNdNd;
trajec = topology.trajec;
p_ac = topology.ac;
[S, ~] = size(p_ac);
[~, ~, M] = size(trajec);
L = 4;

%--------------------initialize vectors-------------------------------------------------
C = zeros(L,L,M,N+1,2);
x_p = zeros(L,M);
C_p = zeros(L,L,M);
x = zeros(L,M,N+1,2);
ndist_ndac = measurements.NdAc;
ndist_ndnd = measurements.NdNd;
measurements = cell(M,1);

for t=1:T
    for m=1:M
        x(:,m,1,t) = meanInit(:,m);
        C(:,:,m,1,t) = covInit(:,:,m);
    end
end

%--------------------iterate over all time steps----------------------------------------
for n = 1:N
    
    %--------------------perform message passing iterations-----------------------------
    for t = 1:T
        idx = -mod(T,2) + 2;
        idx1 = mod(t,2) + 1;
        idx2 = -mod(t,2) + 2;        
        
        if (t == 1)
            for m = 1:M
                
                %--------------------perform prediction---------------------------------
                x_p(:,m)  = A*x(:,m,n,idx);
                x(:,m,n+1,idx1) = x_p(:,m);
                C_p(:,:,m) = A*C(:,:,m,n,idx)*A' + W*diag([var_v var_v])*W';
                C(:,:,m,n+1,idx1) = C_p(:,:,m);
                
                %--------------------stack measurements---------------------------------
                y = zeros(sum(con_ndac(m,:))+sum(con_ndnd(m,:)),1);
                k = 0;
                for i=1:S
                    if( con_ndac(m,i) )
                        k = k + 1;
                        y(k) = ndist_ndac(m,i,n);
                    end
                end
                
                for i=1:M
                    if( con_ndnd(m,i) )
                        k = k + 1;
                        y(k) = ndist_ndnd(m,i,n);
                    end
                end
                measurements{m} = y;                
            end
        end
        
        %--------------------iterate over all mobile agents-----------------------------
        for m = 1:M            
            y = measurements{m};            
  
            L_s = L*(sum(con_ndnd(m,:))+1);
            kappa_s = 3 - L_s;
            lambda_s = alpha^2*(L_s + kappa_s) - L_s;
            X_s = zeros(L_s,2*L_s+1);
            x_s = zeros(L_s,1);
            C_s = zeros(L_s);
            
            x_s(1:L) = x_p(:,m);
            C_s(1:L,1:L) = C_p(:,:,m);
            k = 0;
            for i=1:M
                if(con_ndnd(m,i) == 1)
                    k = k + 1;
                    x_s(L*k+1:L*k+L) = x(:,i,n+1,idx1);
                    C_s(L*k+1:L*k+L,L*k+1:L*k+L) = C(:,:,i,n+1,idx1);
                end
            end
            
            X_s(:,1) = x_s;
            X_s(:,2:L_s+1) =  repmat(x_s,1,L_s) + sqrt(L_s + lambda_s)*sqrtm(C_s);
            X_s(:,L_s+2:2*L_s+1) = repmat(x_s,1,L_s) - sqrt(L_s + lambda_s)*sqrtm(C_s);
            
            Y = zeros(sum(con_ndac(m,:))+sum(con_ndnd(m,:)),2*L_s+1);
            k = 0;
            for j = 1:S
                if(con_ndac(m,j))
                    k = k+1;
                    for i = 1:2*L_s+1
                        Y(k,i) = sqrt((X_s(1,i) - p_ac(j,1))^2+(X_s(2,i) - p_ac(j,2))^2);
                    end
                end
            end
            
            p = 0;
            for j = 1:M
                if(con_ndnd(m,j))
                    k = k+1;
                    p = p+1;
                    for i = 1:2*L_s+1
                        Y(k,i) = sqrt((X_s(1,i) - X_s(p*4+1,i))^2+(X_s(2,i) - X_s(p*4+2,i) )^2);
                    end
                end
            end
            
            y_p = lambda_s/(L_s + lambda_s)*Y(:,1);
            for i = 1:2*L_s
                y_p = y_p + 1/(2*(L_s + lambda_s))*Y(:,i+1);
            end
            
            P_yy = (lambda_s/(L_s + lambda_s) + 1 + alpha^2 + beta) * (Y(:,1) - y_p) * (Y(:,1) - y_p)';
            for i = 1:2*L_s
                P_yy = P_yy + 1/(2*(L_s + lambda_s))*(Y(:,i+1) - y_p) * (Y(:,i+1) - y_p)';
            end
            
            C_m = diag(var_m*ones(length(P_yy),1));
            P_yy = P_yy + C_m;
            
            P_xy = (lambda_s/(L_s + lambda_s) + 1 + alpha^2 + beta) * (X_s(:,1) - x_s) * (Y(:,1) - y_p)';
            for i = 1:2*L_s
                P_xy = P_xy + 1/(2*(L_s + lambda_s))*(X_s(:,i+1) - x_s) * (Y(:,i+1) - y_p)';
            end
            P_xy = P_xy(1:L,:);
            
            N_mess = sum(con_ndac(m,:))+sum(con_ndnd(m,:));
            
            K = P_xy/P_yy;
            x(:,m,n+1,idx2) = x_p(:,m) + K*(y(1:N_mess) - y_p);
            C(:,:,m,n+1,idx2) = C_p(:,:,m) - K*P_yy*K' ;
        end
    end
end
xOut = x(:,:,:,idx2);

%--------------------calculate MSE------------------------------------------------------
mse = zeros(N+1,M);
for m=1:M
    tmp = x(:,m,:,idx2);
    for i=1:N+1
        mse(i,m) = norm(trajec(:,i,m)-tmp(:,i))^2;
    end
end

end